import sys
from time import sleep
from leapc_cffi import libleapc, ffi
import math

# 物体检测

class ObjectTracker:
    def __init__(self):
        self.connection = ffi.new("LEAP_CONNECTION *")
        self.last_position = None
        
    def initialize(self):
        result = libleapc.LeapCreateConnection(None, self.connection)
        if result != 0:
            print("创建连接失败")
            return False
            
        result = libleapc.LeapOpenConnection(self.connection[0])
        if result != 0:
            print("打开连接失败")
            return False
            
        return True
        
    def get_tracking_frame(self):
        tracking_event = ffi.new("LEAP_TRACKING_EVENT *")
        result = libleapc.LeapPollConnection(self.connection[0], 
                                           ffi.sizeof("LEAP_CONNECTION_MESSAGE"),
                                           ffi.new("LEAP_CONNECTION_MESSAGE *"))
                                           
        if result != 0:
            return None
            
        result = libleapc.LeapGetFrame(self.connection[0], tracking_event)
        if result != 0:
            return None
            
        return tracking_event
        
    def calculate_direction(self, current_pos):
        if self.last_position is None:
            self.last_position = current_pos
            return None
            
        dx = current_pos[0] - self.last_position[0]
        dy = current_pos[1] - self.last_position[1]
        dz = current_pos[2] - self.last_position[2]
        
        self.last_position = current_pos
        
        abs_dx = abs(dx)
        abs_dy = abs(dy)
        abs_dz = abs(dz)
        
        threshold = 5  # 移动检测阈值
        if max(abs_dx, abs_dy, abs_dz) < threshold:
            return "静止"
            
        if abs_dx > abs_dy and abs_dx > abs_dz:
            return "向右" if dx > 0 else "向左"
        elif abs_dy > abs_dx and abs_dy > abs_dz:
            return "向上" if dy > 0 else "向下"
        else:
            return "向前" if dz < 0 else "向后"
            
    def run(self):
        if not self.initialize():
            return
            
        print("开始追踪物体移动...")
        try:
            while True:
                frame = self.get_tracking_frame()
                if frame:
                    # 获取点云数据中的质心位置
                    if frame.tracking_frame_id > 0:
                        # 使用第一个检测到的物体的位置
                        position = (
                            frame.pHands[0].palm.position.x if frame.nHands > 0 else 0,
                            frame.pHands[0].palm.position.y if frame.nHands > 0 else 0,
                            frame.pHands[0].palm.position.z if frame.nHands > 0 else 0
                        )
                        
                        if any(position):  # 如果检测到物体
                            direction = self.calculate_direction(position)
                            if direction:
                                print(f"移动方向: {direction}")
                
                sleep(0.1)  # 控制刷新率
                
        except KeyboardInterrupt:
            print("\n停止追踪")
        finally:
            libleapc.LeapCloseConnection(self.connection[0])

if __name__ == "__main__":
    tracker = ObjectTracker()
    tracker.run()